package com.googlecode.gunncs.sensor;

import java.awt.Color;
import java.util.ArrayList;

import javaclient3.RangerInterface;
import javaclient3.structures.PlayerPose3d;

import com.googlecode.grtframework.core.GRTLoggedProcess;
import com.googlecode.gunncs.event.RangeFinderEvent;
import com.googlecode.gunncs.event.RangeFinderListener;
import com.googlecode.gunncs.graph.TimedLinePlot;

/**
 * A RangeFinder publishes RangeFinder events
 * 
 * @author ajc
 * 
 */
public class RangeFinder extends GRTLoggedProcess {

	private static final Color[] colors = new Color[] { Color.red,
			Color.orange, Color.yellow, Color.GREEN, Color.blue };

	private final RangerInterface ri;
	private double[] sonarValues;
	private PlayerPose3d[] poses;
	private double[] angles;
	double[] previousValues;
	TimedLinePlot ranges;

	private ArrayList<RangeFinderListener> listeners;

	public RangeFinder(RangerInterface ri) {
		super("ranger");
		this.ri = ri;
		// poses = ri.getGeom().getPoses();
		// ranges = new TimedLinePlot("ranges", 100);
		// ranges.setYRangingFixed(0, 10);
		// for (int i = 0; i < 5; i++) {
		// ranges.addData("" + i, colors[i], "?");
		// }

		while (!ri.isDataReady()) {
			try {
				Thread.sleep(50);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		previousValues = new double[ri.getData().getRanges().length];
		listeners = new ArrayList<RangeFinderListener>();

		ri.queryGeometry();
		while (!ri.isGeomReady()) {
			try {
				Thread.sleep(50);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		angles = new double[ri.getGeom().getPoses_count()];
		for (int i = 0; i < angles.length; i++) {
			angles[i] = ri.getGeom().getPoses()[i].getPyaw();
		}
	}

	public void run() {
		running = true;

		while (running) {
			if (!suspended) {
				if (ri.isDataReady()) {
					sonarValues = ri.getData().getRanges();

					for (int i = 0; i < sonarValues.length; i++) {
						// if (previousValues[i] != sonarValues[i]) {
						notifyListeners(i);
						// }
					}

					// for (int i = 0; i < 5; i++) {
					// // ranges.addPoint("" + i, sonarValues[i]);
					// // ri.getGeom().getPoses()[0].getPyaw();
					// RangeFinderEvent ev = new RangeFinderEvent(this, ri
					// .getGeom().getPoses()[i].getPyaw(),
					// sonarValues[i]);
					//
					// for (RangeFinderListener l : listeners) {
					// l.distanceChanged(ev);
					// }
					// }

					previousValues = sonarValues;
				}
			}

			try {
				Thread.sleep(50);
			} catch (InterruptedException e) {
				e.printStackTrace();
			}

		}
	}

	public void notifyListeners(int id) {
		RangeFinderEvent ev = new RangeFinderEvent(this, id, angles[id],
				sonarValues[id]);
		for (RangeFinderListener l : listeners) {
			l.distanceChanged(ev);
		}
	}

	public void addListener(RangeFinderListener l) {
		listeners.add(l);
	}

	public void removeListener(RangeFinderListener l) {
		listeners.remove(l);
	}

}
